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ABSTRACT 

The author has formulated a new, general model for specifying the kinematic properties 
of serial manipulators. The new model kinematic parameters do not suffer 
discontinuities when nominally parallel adjacent axes deviate from exact parallelism. 
From this new theory the author develops a first-order, lumped-parameter, calibration- 
model for the ARID manipulator. Next, the author develops a calibration methodology 
for the ARID based on visual and acoustic sensing. A sensor platform, consisting of CjF 
camera and four sonars attached to the ARID end frame, performs calibration 
measurements. A calibration measurement consists of processing one visual frame of 
an accurately placed calibration image and recording four acoustic range measurements. 

A minimum of two measurement protocols determine the kinematics calibration-model 
of the ARID for a particular region: assuming the joint displacements are accurately 
measured, the calibration surface is planar, and the kinematic parameters do not vary 
rapidly in the region. No theoretical or practical limitations appear to contra-indicate 
the feasibility of the calibration method developed here. 
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1.0 PROJECT DESCRIPTION 

Parameter and assembly tolerances and non-rigid-body effects due to link and 
joint flexion and thermal expansion introduce errors into the tool pose at the end- 
effector of a robot. Accounting for the various sources of kinematic error by 
means of a lumped, first-order model allows one to compensate for pose errors at 
different configurations. By calibrating the robot at discrete poses, designated 
calibration poses , in the region of its workspace where it will see service, one 
increases the robot's overall accuracy at those poses. The error-model will then 
permit interpolation between calibration poses to increase the accuracy of the 
robot at non-calibration poses. 

The primary goal of this project is to increase the operational accuracy of the 
ARID robot through calibration and adjustments in the ARID control software. 

Success in calibration hinges on the performance of the the following tasks, 

1. Develop an appropriate kinematics model of the ARID robot to account for 
the observed end-frame positioning errors. 

2. Identify a measurement technique for accurately determining the poses of 
the ARID robot. 

3. Determine the critical calibration poses for measurement. 

4. Measure ARID positioning errors at the critical calibration poses.. 

5. Develop, interface and test a computer program that will use 
experimentally measured data to compensate for ARID's inaccuracies. 

Points 4 and 5 are not addressed in this paper. 


2.0 SUMMARY OF PROGRESS 

* -- 

During this program the author has 

1. Formulated a kinematics model for calibration, 

2. Developed a calibration methodology for the ARID 

3. Measured the angular error about the x- and y-axes of the ARID end-frame for 
variety of configurations. This effort is given in another report [1]. 
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Work yet to be done, 

1. Complete the derivation of the ARID calibration-model and devise a methodology 
for solving for the error parameters. 

2. Configure the Perceptics NuVision system and write C-code to perform the 
required blob analysis on the proposed calibration square. 

3. Design and configure a calibration sensor-platform consisting of a camera and 
four sonar sensors. 

4. Write software to acquire visual and acoustic data from the sensor platform. 

5. Implement the calibration algorithms in software. 

6. Test the calibration sensor-platform on a calibration square lying on a 
microscopically flat table. 

7 . Compute the ARID calibration model from calibration measurements. 

8. Test and verify the derived calibration model. 

In the next section, the author develops a general calibration model with general 
applicability. 


3. KINEMATICS MODEL FOR CALIBRATION 


A lumped kinematics model for calibration has been devised. The lumped- 
parameter calibration model is given in Table 3.1. The parameters found in the 
table are defined in Fig. 3.1. This model's parameters do not depend on the 
common normal between two axes, hence, the calibration parameters do not 
suffer the discontinuities incurred by the Denavit-Hartenburg parameters with 
nominally parallel axes. 


Table 3.1 : Ca 


ibration Kinematic Parameters for the ARID Robot 


Joint 

d 

e 

P 

a 

a 

Joint Limits 

1 P 

di 

ei 

Pi 

a i 

a i 

[O/Vic/ies, IWnches] 

2 r 

0 

e 2 

p2 

a 2 

a 2 

[4° , 112°] 

3 r 

0 

e 3 

P3 

a 3 

<*3 

[102° , 148°] 

4 r 

0 

04 

0° 

0 

0° 

[-16°, -117°] 
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The angle parameters 6, (3, a, will be called the joint-angle, link pitch and link 
Mist , respectively. The parameter d is the link offset and a the link length . 
These parameters are determined as follows. 



Figure 3.1 Calibration-model kinematic parameters between adjacent joint axes 
of a serial manipulator. 


Draw a perpendicular line from the origin of frame F j.j to the z, -axis with 
dj = 0. The intersection of this perpendicular line with the z, -axis defines the 
origin of the next link frame, F, . The length of this perpendicular line equals a, . 
For revolute joints c/, always equals zero. For prismatic joints, d equals the 
joint displacement. Thus, for revolute joints, this model has the same number of 
parameters as the DH-model, namely, four. With prismatic joints, however, this 
model requires a fifth parameter, the link offset d { . 

The angle 0 equals the amount of rotation about the z f .y -axis required to make 
the z,.y -axis , the -axis and the line a, coplanar. Rotating about the new y- 
axis aligns the new x-axis to the line a, . Translation along the line a, moves the 
frame to the origin of F, , To finish the alignment, twist the moving frame z-axis 
about the line a, by the angle a, to align it with the z, -axis . 
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In summary, the sequence of coordinate transformations between successive joint 
frames to generate the link transformation Lj consists of the following: 

Revolute Joint: 

Lj := Rotated 0 , ) Rotated- A ) Translated, a, ) Rotated, a i ) 

Prismatic Joint: 

Lj := Translated d t ) Rotated 0,) Rotated- A) Translated- «,•) 
Rotated- a, ) 

The link transform, therefore, computes to 

L * := _ 0 0 0 1 ]’ 

where 




c Pi c 0i 

c 0i s ai s (ii “ c ai ^0i 

c ai c 0i s Pi s ai s 0i 

c pi s 0i 

c ai c 0i + ^ai Spi ^0i 

“C0i S a j + C a j Spj Sqj 

-spi 

c Pi ^ai 

^ai c pi 


(3-2) 


and 


a > cpj c ei 

a i c pi s 0i 
di - aj Spj 


(3-3) 


The full forward kinematics °T 4 for the ARID equals 

°T 4 = Lj Yj2 L 3 L 4 . (3-4) 

Symbolic computation of the complete expression for °T 4 probably exceeds 
practical requirements. Since each link twist a, and link pitch A lie close to 

zero, second-order and higher terms in these variables may be ignored. °T 4 will 
be computed only to first order terms in the a, and A • This computation 
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appears in Section 3.2. Before continuing further, it will be instructive to define 
the various coordinate frames involved in this report. 

3.1 Identifying the Various Coordinate Frames 

Figure 3.2 indicates the relative orientation of the principle coordinate frames 
discussed in this report. The camera frame is defined at the lens center and the 
image frame on the image plane. Each calibration square on the calibration strip 
defines a calibration frame. The ARID base frame follows from the standard link 
frame definitions. Motion along the ARID base frame z-axis , therefore, 
corresponds to moving along the Orbiter’s x-axis. To simplify the ensuing 
discussions, the term ARID in this report refers to the ARID on the Orbiter's 
starboard side. 



Figure 3.2 Depiction of the image, camera, calibration, ARID and Orbiter 
frames. 


From Fig. 3.2 one determines that the rotation °R or biter , 
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°R 


orbiter 


o-io 
0 0-1 
1 0 0 


transforms Orbiter coordinates into Starboard-ARID base frame coordinates. 


3.2 First-Order Calibration Model for the ARID 


For manipulators where adjacent joint axes are nominally parallel, the link 
parameters a, and /?, will be approximately zero. To first order, 


spj := sin(j3, ) « ft , s ai := sin(a, )~ a, , c pi := cos(/3 f ) = f , c^ := cosa, ) « 1, 
and s^ s pi = 0. With these approximations the link transform rotation becomes 

c 0i — s 0i Pi c 0i "F ^0i 

sei c ei -aiC 0i +PiS ei j (3.5) 

-Pi aj 1 


Ri = 


and 


0i ^0i 


a i s 0i 

di - P i a j 


(3-6) 


Suppose links one and two have nominally parallel joint axes, then the first-order 
approximation of 


Li L 2 


R i R2 R1I2 + I1 
0 0 0 1 


(3-7) 


consists of 
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Ri R 2 - 


C12 


- si; 


S12 C12 

"Pi C2+OC1 S2 — P2 Pi S2+CX1 C2+CX2 


P2 Cl 2 +ot 2 s 12 + Pi Cl +«1 Si 

P 2 Sl2 _ 0t2 Cj2 + Pi S|— ai Ci 
1 


(3-8) 


and 


Ri h + h 


a 2 ci2 + ai ci + d 2 (pi Ci+aiSi) 

»2 s 12 + ai sj + d2<Pi Si-ai Ci) 

di + d 2 + a 2 (-pi c 2 +ai s 2 - P2) - Pi 3i 


(3-9) 


Since the joint axes of the ARID are all nominally parallel, its first order 
calibration model may be computed using the general results in (3-8) and (3-9) to 
assist in the calculation of °T 4 = Li L 2 L 3 L 4 . The results equal 


°T 4 


c 1234 

- s 1 2 3 4 

£1 

a l C 1 + a 2 c 12 + a 3 c 1 23 

s 1234 

c 1 234 

£2 

a l S 1 + a 2 S12 + a 3 S123 

£3 

£4 

1 

di + £5 

0 

0 

0 

1 


(3-10) 


where the first-order error terms 81 , 82 , 83 , 84 , 85 are given by 

El = P4C1234 + (X4S1234 + P3 Cl23 + S123 + P2C12 + CX2S12 + Pi Cl + (Xi Si (3-1 la) 

82 = P4 S 1234 - &4 C1234 + p3 S123 — 0I3 C123 + P2 S12 - <*2 C12 + Pi Si — ai C] ( 3-1 lb) 

03 = — Pi c 2 34 + oti s 2 34 — P2 C34 + a 2 S34 — p 3 c 4 + (X3 S4 - P4 ( 3 - 1 1 c) 

£4 = Pi s 2 34 + ai c 23 4 + P 2 S34 + a 2 C34 + P 3 S4 + (X3C4 + (X4 ( 3-1 Id) 

85 = di + a 3 (— Pi c 2 3 + ai s 2 3 — P 2 c 3 + a 2 S 3 — P 3 ) + a 2 (— pi c 2 + oti s 2 — p 2 ) — P 1 a j 

(3-1 le) 
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Equation (3-11) may be written more succinctly, 

e = E\| f, (3-12) 

where 

e := [El E2 £3 E 4 £5 “ dl] T > V := [Pi P2 P3 P4 «1 «2 «3 a 4 ] T . 
and 
E = 


C1234 

C123 

C12 

Cl 

S1234 

S123 

Sl 2 

si 

S1234 

S123 

S12 

Si 

-C1234 

-Cl 23 

-Cl 2 

—Cl 

-C 234 

-C34 

-C 4 

-1 

S234 

S34 

S 4 

0 

S 234 

S34 

s 4 

0 

C 234 

C 3 4 

C 4 

1 

- a 2 C 2 - a 3 C 2 3 

-a 2 - a 3 c 3 

-33 

0 

a2S 2 + a 3 s 23 

33 S3 

0 

0 


Observe that E only depends upon the joint angles 6t and the link lengths a*. 


3.3 Solving for the ARID Kinematic Parameters 

Let °M 4 be a measurement of the ARID end-frame pose expressed in the base 
frame. The calibration problem is to solve for the unknown kinematic parameters 
in °T 4 of (3-10), given one or more measurements: °M 4 = °T 4 . At this juncture 
one must determine which kinematic parameters are known. For example, if dj, 
the link lengths a, , and the joint angles 0, are known with a high degree of 
precision, one need only solve for in (3-12). Since \|/ contains eight unknowns, 
the system is under-determined for one measurement and over-determined with 
two measurements, assuming of course, that the /J, and a, do not change 
significantly for the two robot configurations in which the measurements are 
made. 

If all the kinematic parameters are unknown, the problem becomes non-linear. 
One can use the inverse kinematics solutions to express the sines and cosines of 
the revolute joint angles in terms of rational polynomials in the link lengths. The 
last column in °M 4 = °T 4 will then provide two equations in the three unknowns, 
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ai, a 2 , and 83 . Two measurements will yield four equations in the three link 
lengths. A least- squares solution can be employed to solve for them. With the link 
lengths determined, the joint angles for the two measurements may be calculated. 
Finally, one may compute y , given that dj has been calibrated separately. This 
procedure requires that the kinematic parameters do not vary significantly for the 
two measurement configurations of the robot. 

The rest of this paper deals with how to generate the measurement poses °M 4 . 

4. Calibration Measurement Technique 

The approach proposed here is to mount a digital camera and four sonic sensors 
onto die end-effector flange of the ARID robot with approximately the same total 
mass as the Lockheed end-effector. Refer to Figure 6.1 for a stylized side view of 
this calibration end-effector. 

The camera pose with respect to the flange must be known. The camera will view 
a calibrated strip (Fig. 5.2) of four-inch squares with appropriate fiducial 
markings (Fig. 5.1). The calibration strip will run along the Orbiter y-axis on a 
partial, bay-door mock-up (Fig. 5.3). The strip permits a visual calibration 
measurement every four inches for a total of 28 measurements along the bay 
door. For high curvature areas of the bay-door, calibration might be required 
every one or two inches, particularly near the hinges. The author does not expect 
more than two or three such one-inch squares will be needed. Most calibration 
squares will be 4 inch squares. The number of calibration strips required will 
have to be determined by experiment. 

A frame-grabber will be needed to capture the image and simple vision software 
to compute the gray level centroid for each fiducial mark. From these centroids 
the camera x-y position and orientation with respect to the image can be 
computed. 


The four sonar sensors, arranged in an I pattern about the camera, compute the 

pitch and yaw of the camera frame and range data provides the distance from the 
camera center to the image center. 

The sonar and vision measurements yield a total of six parameters and uniquely 
determines the pose of the camera, hence, the end-effector flange frame. 
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5. Vision Measurement 

5.1 Details of the Calibration Square 

Figure 5.1 illustrates one proposed calibration square. Each square contains five, 
solid fiducial squares \ " x \ " and a center fiducial 2 " x 4 " • Th e centroids of 

these fiducial marks constitutes the critical information proffered by the 
calibration square. The fiducial in the upper left-hand-comer of the calibration 
square uniquely identifies quadrant two. Unless the robot end-effector pose 
deviates more than several inches, this fiducial mark will provide redundant 
information. 



Figure 5.1 Typical visual calibration square. 

The centroids of the remaining four perimeter fiducials determine the orientation 
and center of the calibration square with respect to the camera image. The upper- 
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center and lower-center fiducial centroids determine the vertical axis of the 
calibration square while the left-center and right-center fiducial centroids 
determine the horizontal axis of the calibration square. The centroid of the center 
fiducial marks the center of the calibration square and should be at the 
intersection of the vertical and horizontal axes determined by the other four just 
mentioned. The y-axis parallels the long side of the center fiducial and the x-axis 
parallels the short side. 

The center fiducial, having twice the area of the other fiducials will be easy to 
identify in a camera image. From the center fiducial one can easily identify the 
others. In theory, the central fiducial yields all the information about the position 
and orientation of the calibration square. The redundant fiducials provided on the 
calibration square increase the the accuracy of the measurement. 

Since the measurement of a fiducial centroid depends upon an area measurement, 
the computation will be robust to noise. Isolated white pixels in a black fiducial 
square will not significantly affect the computation of the fiducial’s centroid. 

For a 512 x 512 pixel camera, if the calibration frame fills the field of view, 

4 inches 1 

each pixel will correspond to a displacement of ^ ^ = inch « 0.0078 

inches. 


5.2 The Calibration Strip 

A calibration strip consists of a sequence of concatenated calibration squares 
drawn on vellum or some other durable, thin, compliant material (Fig. 5.2). This 
strip, placed upon a mock-up segment of the bay door surface (Fig. 5.3) will 
permit visual calibration of the ARID robot at prescribed intervals. For instance, 
the ~112" width of the orbiter bay door divides into 28 four inch intervals. A 
calibration strip would consist of 28, four-inch squares. At the hinge-end of the 
bay door the sharp curvature over four inches may require adjustments to the 
sonar range measurement. This will be discussed in the section on sonar 
measurements. 
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Figure 5.2 Visual calibration strip for the ARID robot. 
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Figure 5.3 Layout of the four inch wide calibration strip. Indexes mark four 
inch intervals on the bay door. 


5.3 Image Measurements 

Figure 5.4 depicts the image frame x'-y'-z', the camera frame x-y-z, and the 
calibration frame x c -y c -z c . The camera coordinates of an object point 

p = [x y z] T on the calibration strip and its image point p' = [x’ y' f ] T 
satisfy the projection equations 


x ~ _ x 
f — z 


and 


y: _ y 

f "z • 


The vision system yields the image coordinates x' and y' and the focal length / of 
the camera. The camera z-coordinate of the object point can be determined a 
number of ways. Section 7.4 derives an expression for x,y,z based upon the 
image data and the acoustic measurements performed by.the four sonar sensors. 
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Figure 5.4 Depiction of the image, camera, calibration, ARID and Orbiter 
frames. 

In Fig. 5.5 the camera visual field includes fiducial points p 3 ,p 4 ,p 6 of one 
calibration square and points pi and pi of another. The image coordinates p.’ of 

these fiducial points, along with range information from the sonars, allow us to 
compute their camera-coordinates pi (Section 7.4). The camera field-of-view 
must be large enough to include at least three non-collinear fiducials in order to 
determine the rigid-body transformation between the camera frame and the 
calibration frame. For example, in Fig. 5.5 the coordinates of p 3 , p 4 and p 6 yield 
sufficient information to do that. The vectors 


a := [a x a y f = p 4 - p 6 and b := [b x b y } x = p 4 - p 3 

9 b 

define the coordinate frame axes vectors x c = and y c = ^ of the calibration 

square with respect to the camera frame. The third axis equals the cross-product 
of the other two: z c = x c xy c The vectors x c ,y c ,z c should be orthogonal. 

Measurement error may cause x c y c * 0, hence, the matrix 
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Cam " a RcaIibration = Jc *cl. 

Figure 5.5 Camera image of a calibration square. 
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which transforms calibration-coordinates into camera-coordinates, may not be a 
true rotation as it would be with exact measurements. In any case, the measured 
rigid-body transformation which converts calibration-coordinates to camera- 
coordinates equals 


camera 


^calibi 


ration 


camera™ 

^calibration 

0 0 0 


P4 

I 


With °T 4 equal to the forward transform of the ARID and 4 T camera equal to the 
fixed, rigid-body transformation relating the camera-frame to the ARID end- 
effector frame, we have the identity, 

0v* _ (Vp 4rp camera** 

1 'calibration '4 1 camera ^'calibration 

where °M calibration relates the ARID base-frame to the coordinate system of the 
calibration square under view. The above relationship yields one set of equations 
in the unknown parameters for each calibration pose measured. Section 8.0 will 
elaborate the procedure. 

5.4 Fiducial Identification Algorithm 

The above computation hinges on the assumption that the fiducial marks can be 
identified. Refer to Figure 5.6 in following the logic of the FID algorithm 
below. The assumption is that a center fiducial always appears in the camera 
field-of-view. Assume there are n pixels per inch. 
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Figure 5.6 A typical calibration square with fiducial centroids indicated by /?, . 

FID Algorithm 

1. Scan image left-to-right, top-to-bottom, starting at the upper left-hand- 

n 2 

comer until a blob is encountered with area: ~g~ . The centroid of that blob 
equals the center point p 4 . The longest side of the center blob determines 
the y-axis. The direction of the y-axis will usually be towards increasing y’ 
of the image frame. For this not to be the case the ARID would have to be 
in error by 180°. 

2. Determine the centroid and the midpoint of two adjacent edges of the 
center blob. The lines from the centroid to the two edges provide a rough 
estimate of the x- and y-axes of the calibration-frame in which the fiducial 
is located. 

3. From the center fiducial search for the perimeter fiducials approximately 
1.75 n pixels along the four directions found in Step 2. Find the centroid 

n 2 

of any blob with area equal to yg and compute the longest vector along 
each axis to increase the accuracy of the axes measurements. 
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6. ACOUSTIC MEASUREMENTS 

The acoustic sensor measures four perpendicular distances hi , i = 123,4, to the 
calibration strip (Fig. 6.1, also refer to Fig. 7.1). Section 7.0 illustrates how the 
acoustic measurements determine the plane of a calibration square, assuming, of 
course, that the bay door is locally flat. The curvature along the Orbiter’s y-axis 
requires the sonar separation to be small compared to the curvature. Since we 
desire to calibrate the ARID every four inches, and four inches is small compared 
to the bay door curvature (except near the hinges), one solution would be to make 
the sonar separation d along the y-axis equal to four inches, if this is possible. 
The Orbiter is quite flat along the x-axis , so the separation of the sonar sensors 
can be further apart along that axis. In the ensuing discussion the distance 
between the two x-axis sonars will be assumed equal to the distance between the 
two y-axis sonars. This distance will be denoted by d. 



Orbiter Bay Door Profile 

Figure 6.1 Side view of the calibration end-effector showing the camera and 
two of the four acoustic sensors. 

We emphasize that the sonar sensors must scan a locally flat surface. Should this 
not be possible on an Orbiter bay door mock-up, the calibration strip can be 
placed on a flat surface and the ARID calibrated on that surface. From 
preliminary experiments with the ARID, such a calibration procedure might 
actually work better than using an Orbiter bay door mock-up section. 

The integration of the sonar measurements with the vision imaging to locate the 
calibration square fiducial points in camera-frame coordinates will be described 
next. 
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7. CAMERA COORDINATES OF CALIBRATION POINTS 

The goal of this section is to compute the camera-frame coordinates of the point 
p = p(x,y,z) on the calibration strip, given 

1 . The absolute distance between pixels, 

2. The focal length/ of the camera, 

2 The distance between the sonar sensors 2 y, 

3. Sonar measurements /i, , i = 1,2J,4, and 

4. The camera-frame coordinates of its image point P' = P(x'yf). 

Figure 7.1 depicts the calibration end-effector taking a measurement. The sonar 
sensors measure the heights /?, , i = 1 ,2,3,4, and the camera takes an image of the 
calibration strip. The camera has focal length/. The distance between the sonar 
sensors d = 2 y. The camera-frame origin coincides with the lens center and the 
optical axis coincides with the the frame's z-axis. 

Since the fiducial points on the calibration strip have known positions, the 
computed coordinates of a fiducial point p can be compared to its known value, 
provided one can identify that fiducial. We treated the problem of identifying a 
specific fiducial in Section 5.4. 


Measurement of the position of any three, non-collinear, fiducial points 
completely determines the plane of the calibration strip, the calibration-plane , 
with respect to the camera-frame and, therefore, provides a complete calibration 
measurement. 

The acoustic measurements yield the positions of the points p, , i = 1 ,2,3,4, on the 
calibration-plane. Four points over-determine the plane. Since the orbiter bay 
doors are not exactly flat and the acoustic measurements may not, in any case, 
precisely place the four points on a common plane, one may wish to find a plane 
that "best-fits" the four points. In this case, one may wish to employ the Moore- 
Penrose inverse to compute the unique plane that fits the data in a least-squares 
sense. 

From Fig. 7.1 one can determine the camera-frame coordinates of the point p f , 
i - 1,2, 3, 4, in terms of the acoustically measured heights h , , i = 1,2, 3, 4, and 
the between-sensor distance d = V2 y. 
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Figure 7. 1 Acoustic and visual measurement parameters 

Consider the detail of the plane determined by the parallel lines h j and /i* in 
Fig. 7.2. One expects the acoustic sensor to measure the perpendicular distance 
to the calibration plane since the reflection along the perpendicular travels the 
shortest path. Experimental observation appears to verify this expectation. 


Camera Plane 



Bay Door Surface or Calibration Surface 


Figure 7.2 Acoustic and visual measurement parameters 
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The acoustic measurements hi and h 4 allow us to compute the camera coordinate 
z-components of the points pj and p 4 . In Fig. 7.2, the distance d^ = d 42 , 
hence, 


sin(oCc) = h d 4 ^ h l . (7-1) 

The camera z-coordinates of the points pj and p 4 compute to 

Z - ~ COS(CXc) ^ 24 “ cos(oCc) ’ (7-2) 

hence, the camera coordinates of these two points equal, respectively, 



-Y 


-Y 


-Y 


Y 

Pl = 

-h, 

> P4 = 

-I 14 


COS(CXc) 


COS(Oc) 


A similar consideration of the plane determined by the parallel lines hi and hj 
permits expressing the camera coordinates of points p 2 and ps , 


Y 


Y 

-Y 


Y 

-h 2 

• P3 = 

-h 3 * 

_ cos(p c ) _ 


_ COS(pc) _ 


(7-4) 


In this case, the angle (3 C is determined by 

sin(pc) = . (7-5) 

These values assumes the sonar measurements determine the perpendicular 
distance to the calibration strip. 
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7.1 Computing the Calibration-Plane Equation 

From the coordinates of any three of the four points p, , i = 1,2, 3, 4, one can 
compute the coefficients {r| x , ri y , r| z , % } in the calibration-plane equation, 

TUx + Ti y y + Ti z z + £ = 0, (7-6) 

or, equivalently, 


£ x+ y + ^ Z = -1. (7-7) 

Substituting the coordinates of p , , i = 1,2 J, into the above equation leads to the 
linear system in the unknown coefficients, 


A v = 


where 


A = 


-Y -Y 
Y -Y 


i 

cos(Oc) 

~^2 


cosi 


(Pc) 



1 

X 

> 

1 

II 

1 

X 

1 

1! 

> 

v y 

*ly 

1 

- v z _ 

1 

-=J 

N 

1 


y 

As long as p, , i = 1,2,3, are not collinear, A is invertible and 


v = - A 


-l 


1 
1 

LI J 


determines the coefficients for the plane equation. 


(7-8) 


(7-9) 


(7-10) 
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This method can be applied to all four points as well, except that A becomes 
A. = [pi p 2 P3 P4 ] t , and is no longer invertible. The Moore-Penrose inverse 

(pseudo-inverse) A + , however, can be used to find a plane that fits the data in a 
least-squares sense. The resultant solution v s computes to 



(7-11) 


If three of the four points are not colinear, A has full-column rank and 


A + = (A T A)' 1 A T . (7-12) 

The solution obtained with the Moore-Penrose inverse yields a plane equation that 
fits the data with least-squared-error e, 



" 1 ’ 


" 1 " 

E := | - 

1 

- A v s I 2 < | - 

1 


_ 1 _ 


_ 1 _ 


- A v I 2 


for all v. 


(7-13) 


In other words, no other plane will produce a smaller e than the plane determined 
by the coefficients v s . 


7.2 Alternative Computation of the Calibration-Plane Equation 

The computation of the coefficients of the calibration-plane equation in the 
previous discussion required the inversion of the matrix A. The computational 
requirements are significantly reduced and symbolic solutions obtained by taking 
advantage of the properties of a plane. 

From the coordinates of three, non-collinear points /?,• , i = 1 , 2 , 3 , compute the 
coefficients in the calibration-plane equation, 

r| x x + q y y + ri z z + ^ = 0, (7-14) 

by computing the coefficients of x,y, and z in 

[x y z] • (P 2 -Pi)x (P 3 -Pi) + £ = 0. (7-15) 
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5 = “Pi (P 2 "Pi)x (P 3 -P 1 ). 


(7-16) 


The first term in the above equation is the vector box product, hence, the 
coefficients may also be computed in terms of determinants. For the situation 
depicted in Fig. 7.1, for example, 


2 v 0 - - 1 - - -^r- 

[x y z ] • (p 2 -Pi )x (P 3 -P 1 ) = 1 cos(Otc) cos(p c ) .(7-17) 


2 y 2 y 


COS((Xc) cos(Pc) 


The coefficients of x,y,z in the resulting express for the above determinant equal 


= 2 Ttsfc) • 

To calculate the constant t, evaluate 


)> ’ly= 2 '^p^’ ^ = 4 y 2 - ( 7 ‘ 18 ) 


-Y -Y 


4 = - 2 Y 0 


2 y 2 y 


to obtain 


_±L_ 

COS(Otc) 

hi h 2 

cos(OCc) cos(Pc) 

hi h 3 

cosCCXc) cos(P c ) 


-y -y 


= - 2 Y 0 


0 0 


£ = 2y 2 f _hl_ — hj 
^ r v cos(Oc) cos(Pc) ; ' 


ihj 

COS(Otc) 

hi h 2 

COS(Oc) " COS(pc) 

“hi _ h 3 

COS(Otc) " COS(Pc) 

(7-19) 


(7-20) 


Collecting these results together yields, 
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v, = 


1 


v ( h * + --k-x 
r v cos(a c ) cos(P c ) ; 


( h 2— . ?ll \ 

xos (Pc) cos(Ct c ) ; 

h 3 ~ h 2 


(7-21) 


cos(Pc) 

2 Y 

If the three points p, , i = 7,2,4, had been selected to determine the plane, then 


v 2 = 


1 


h 2 h 4 

^ cos(Pc) + cos(Oc)^ 




cos (Pc) cos(Otc) 

1*4 ~ hi 


cos(CCc) 

2y 


(7-22) 


and 


t = 2v 2 /. "2 , h 4 x 

S r V cos(P c ) cos(CX c )'' " 


(7-23) 


A necessary 
the same, Vj 


and sufficient condition for the two sets of plane coefficients to be 
= v 2 , is that 


hi 

cos(Oc) 


+ 


— hj-_ _ - h 2 + h4 
cos(P c ) cos(p c ) cos(Oc) ' 


(7-24) 


If the measured heights satisfy this condition, then all four points must be on the 
same plane. Of course, with measurements, one cannot guarantee this condition 
will hold and the more complex, least-squared solution may be preferred. 

Now that we have the equation for the calibration-plane, we can use the visual 
data to compute the camera-coordinates of any point p on the calibration-plane 
from the coordinates of its image point p' on the image plane. 


7.3 Camera-Coordinates of a Point on the Calibration Strip 

From geometric optics and the coordinate frame definitions depicted in Fig. 7. 1 , 

the coordinates of p = [x y z] T and its image point p’ = [x’ y’ f ] T satisfy 
the projection equations 

x’ x . y’ y « , 

j = - and \ = (7-25) W 
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In Fig. 7.1 thtz-axis component of the object must always be negative. 

Since every point p on the calibration-plane must satisfy 

v T p = -l, (7-26) 

one can compute the range of the object in terms of its x and y coordinates, 

v* * + v v y + 1 


z = 


-V, 


(7-27) 


Substitute into (7-27) the values for x and y given in (7-25) and solve for z to 
obtain 


-f 


z = 


v x x' + v y y' + v z f • 
Computing x and y with this value of z yields 


(7-28) 


X 

-l 

' x' ' 

.y . 

v x x' + v y y’ + V z f 

. y’ . 


(7-29) 


From V! computed earlier (7-21) and the definitions 


T ^ 1 ' costotc)’ ' cos(Pc)’ ' 


cos 


(Pc) 


, 114 - 


COS(tXc) 


(7-30) 


cos(otc) 


-V 


1 -( 


h 4 - hi 2 


d V 2 


, 


and 


cos(Pc) = a/"i -( h d ~^ 2 ) 


we obtain the camera coordinates x, y of any point in the object plane in terms of 
the acoustic measurements hj , the visual measurements x’ , y’ , and the distance d 
between the sonar sensors, 


X 

-y Cn i ± i) 3 ) 

* x’ 

.y . 

(11 2 - rj i) X' + (t| 3 - 11 2 ) y' + 2 y f 

. y’ . 


(7-31) 


and 
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, =X i (Rl ±M n . m 

(112 - ’ll) X' + (113 - Th) y' + 2 Y f ' 1 

In the next section the camera coordinates of the fiducial points will be used to 
determine the rigid-body transformation between the camera and calibration 
square under view. 

7.4 The Camera-to-Calibration-Frame Transformation 


W 


Assume the fiducial centroids p, , i = 1 ,2,3,4,5, on a calibration square have been 
identified (Fig. 7.3) by the FID algorithm. In Section 7.3 we indicated how the camera 
coordinates x, ,y f , z, of each fiducial centroid p,- can be calculated from acoustic 
measurements and the camera image-coordinates x,\ y- of its image point p - . 
Therefore, the camera-frame coordinates of the calibration-frame of the square under 
view equal 


ip 5 - p 3 i 


. P2-p^ 
lp 2 “ P6 1 


z c = x c x y c • 


(7-33) 


These vectors should be orthogonal. Measurement error may cause x c y c ^ 0, hence, 
the matrix W 


C ”"' n, Rcal i bn,d„.= [*c (7-34) 

which transforms calibration-coordinates into camera-coordinates may not be a true 
rotation as it would be with exact measurements. In any case, the measured rigid-body 
transformation which converts calibration-coordinates to camera-coordinates equals 


camera jyj 


calibration 



camera 


calibration 


0 0 0 


P4 

1 


(7-35) 
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Figure 7.3 A typical calibration square with fiducial centroids indicated by /?, . 

Knowledge of the ARID forward kinematics, the relationship of the camera- 
frame to the ARID end-frame and, now, the camera-coordinates of the fiducial 
points, one can compute the fiducial point coordinates with respect to the ARID 
base frame. Assuming the calibration strip has been accurately positioned with 
respect to the ARID base frame, one can consequently calibrate the ARID at each 
calibration square by comparing the calibrated, fiducial-point, base-frame 
coordinates with the measured fiducial-point, base-frame coordinates. In the next 
section, these ideas will be developed further. 


8. CALIBRATION OF THE ARID 

A_L 

Let M caIibration be a precisely measured rigid-body transformation from the 
Orbiter frame to a calibration square of interest. Define 0rb M 0 to be a precisely 
measured rigid-body transformation from the Orbiter frame to the ARID base- 
frame. The rigid-body transformation 4 M camera , relating the camera frame to the 
ARID end-frame is fixed and assumed known. If this is not the case, a tool-frame 
calibration-model must be appended to the existing ARID model and additional 
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kinematic parameters will need to be determined along with the ARID 
parameters. To simplify the development here, 4 M camera is assumed known. The 

rigid-body transformation camera M calibration (7-35) is calculated from the acoustic 
and visual measurements. 

The rigid-body transformation from the Orbiter frame to the calibration-frame 
must equal 

"Citation = (7-35) 

Solving for °T 4 yields 

°r 4 = “ Bbnld ‘”M cmOT CJmen M 4 (7-35) 

All the transformations on the left-side of (7-35) are known. If we assume the 
joint displacements are accurately known then one can solve for the unknown 
kinematic parameters in °T 4 (3-10), namely, a t , i - 1,2,3, and the eight angle 
parameters in \j / (3-12). Since (7-35) only provides 7 equations, two independent 
measurements will be required. 

From the two measurements eight equations and eight unknowns derive from 
(3-lla,b,c,d). If the resulting 8x8 matrix is well conditioned, then it may be 
inverted to compute \j/ , independently of the linear parameters . In this 
approach (3-1 le) could serve as a partial check of the solution. 

For the ARID, the three linear parameters a { can be solved by a set of equations 

based on the first two entries of the last column of °T 4 . These equations, 
interestingly, do not depend upon the angle parameters in y to first order. 


9. CONCLUSIONS 

A kinematics error-model and an experimental procedure has been proposed for 
calibrating the ARID robot. A camera and sonar end-effector measures the pose 
of the ARID end-frame for different calibration poses. A minimum of two 
measurements determine the kinematics calibration-model of the ARID for a 
particular calibration-square, assuming the joint displacements are accurately 
measured, the calibration surface is planar, and the kinematic parameters do not 
vary rapidly in the region above a particular four inch calibration square. 
Preliminary experiments suggest this is the case. 
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The theoretical developments presented in this paper and the experimental results 
expressed in a companion paper [1], suggest that the ARID manipulator will 
admit of effective calibration. On this basis the author encourages the Robotics 
Section to vigorously pursue implementation of the calibration technique 
described herein. 
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